#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include "kalman.h"
#include "navigation.h"

float getRand(float min, float max) {
	return min + (((float)rand()/(float)RAND_MAX) * (max - min));
}

float accel_x_raw;
float accel_y_raw;
float accel_z_raw;
int accel_read_id;

int main(void) {
	
	
	////////////////////////
	char str1[100] = "$GPRMC,201547.000,A,3014.5527,N,09749.5808,W,0.24,163.05,040109,,*1A\r\n";
	char str2[100] = "$GPGGA,201548.000,3014.5529,N,09749.5808,W,1,07,1.5,225.6,M,-22.5,M,18.8,0000*78\r\n";
	char str3[100] = "$GPRMC,201548.000,A,3014.5529,N,09749.5808,W,0.17,53.25,040109,,*2B\r\n";
	char str4[100] = "$GPGGA,201549.000,3014.5533,N,09749.5812,W,1,07,1.5,223.5,M,-22.5,M,18.8,0000*7C\r\n";
	char *teststrs[4] = {str1, str2, str3, str4};
	
	gps_t gps;
	gps_init(&gps);
	int g,h;
	for (g = 0; g < 4; ++g) {
		char *str = teststrs[g];
		for (h = 0; h < 100; ++h) {
			gps_encode(str[h],&gps);
		}
		printf("%ld %ld\n",gps.latitude,gps.longitude);
		printf("%f\n",gps.course);
		printf("%f\n",gps.speed);
	}
	
	//long a = 24039167;
	//long b = 24039000;
	long a = 63120000;
	long b = 63120001;
	printf("%ld - %ld in meters: %f\n",a,b,diff_lat_meters(a,b));
	
	return 0;
	////////////////////////
	
	///////////////
	// test convert_accelerometer
	kal_t baseLine[3] = {0.2,0.1,1.0};
	kal_t xyc[2];
	kal_t measured[3] = {.2,2.0,1.0};
	kal_t orient = 0.0;
	kal_t vvx = 109.285;
	kal_t vvy = 0.0;
	kal_t ppx = 0.0;
	kal_t ppy = 0.0;
	for (; orient < 361.0; orient += 1.0) {
		//printf("%f %f %f\n",orient, sinf(orient*0.0174532925),cosf(orient*0.0174532925));
		//convert_accelerometer(orient, measured, xyc, baseLine);
		
		vvx += xyc[0];
		vvy += xyc[1];
		
		ppx += .5*xyc[0] + vvx;
		ppy += .5*xyc[1] + vvy;
		
		printf("%f %f %f %f %f %f\n",xyc[0],xyc[1],vvx,vvy,ppx,ppy);
	}
	
	return 0;
	///////////////
	
	
	// input and state information
	kal_t Px[9]       // 3x3 error covariance
	= { 0.144973, 0.234941, 0.083876,
		0.227745, 0.380634, 0.139176,
		0.079498, 0.124546, 0.124546 };
	kal_t Py[9]       // 3x3 error covariance
	= { 0.144973, 0.234941, 0.083876,
		0.227745, 0.380634, 0.139176,
		0.079498, 0.124546, 0.124546 };
	
	kal_t Kx[9];      // 3x3 kalman gain
	kal_t Ky[9];      // 3x3 kalman gain
	kal_t xx[3] 		  // state information {acceleration, 
				// velocity, position}
	= {0.0, 0.0, 0.0 };
	kal_t xy[3] 		  // state information {acceleration, 
	// velocity, position}
	= {0.0, 0.0, 0.0 };
	kal_t yx[3];       // {acceleration, 
		// velocity, position}
	kal_t yy[3];      // {acceleration, 
		// velocity, position}
	kal_t ux[1];       // commanded input {acceleration}
	kal_t uy[1];       // commanded input {acceleration}
	
	
	
	
	int i;
	unsigned long reps = 2600;
	

	
	float d = 0.0;
	float accx0, accy0;
	float velx0, vely0, velx1, vely1;
	velx0 = 0.0, vely0 = 0.0;
	float posx0, posy0;
	posx0 = 0.0, posy0 = 0.0;
	int cnt = 0;
	
	int tw = 0;
	for (; tw < 1; ++tw) {
		float orientation = 0.0;
		float acceleration = 0.0;
		float velocity = 0.0;
		
		int decelerate = 0;
		int stop = 0;
		for (i = 0; i < reps; ++i) {
			if (i >= 900) {
				
				if (i == 300 || i == 900 /*|| i == 1500 || i == 2100*/) {
					cnt = 360;
				}
				if (cnt) {
					--cnt;
					orientation += 0.25;
				}
				
				//printf("%d %d %f\n",i, cnt,orientation);
			}
			
			if (!decelerate && d >= 0.9) {
				decelerate = 1;
				d = 0.9;
			}
			if (decelerate && !stop && acceleration <= 0.0) {
				stop = 1;
				acceleration = 0.0;
			}
			
			if (!decelerate) {
				if (i%2 == 0) 
					d += .05;
			}
			else if (!stop) {
				if (i%2 == 0)
					d -= .05;
			}
			else 
				d = 0.0;
			
			if (i > 2000 && posx0 >= 0.0 && posy0 <= 1.0) {
				d -= 0.05;
			}
			
			acceleration += d;
			velocity += acceleration*time_v;
		
			// radians...
			float ycomponent = sin(orientation*0.0174532925);
			float xcomponent = cos(orientation*0.0174532925);
			
			accx0 = xcomponent*acceleration;
			accy0 = ycomponent*acceleration;
			
			if (cnt) {
				// naive corner at constant velocity modeling
				velx1 = (accx0*time_v + velocity*xcomponent);
				vely1 = (accy0*time_v + velocity*ycomponent);
				
				float d0 = velx0 - velx1;
				float d1 = vely0 - vely1;
				
				d0 *= d0;
				d1 *= d1;
				
				float centripitalAcceleration = sqrtf(d0+d1);
				//printf("%f\n",centripitalAcceleration);
				accx0 = centripitalAcceleration * cos((orientation+90)*0.0174532925);
				accy0 = centripitalAcceleration * sin((orientation+90)*0.0174532925);
				
				velx1 = (accx0 + velx0);
				vely1 = (accy0 + vely0);
			}
			else {
				velx1 = (accx0*time_v + xcomponent*velocity);
				vely1 = (accy0*time_v + ycomponent*velocity);
			}
			
			posx0 = (.5*accx0*time_v*time_v + velx1*time_v + posx0);
			posy0 = (.5*accy0*time_v*time_v + vely1*time_v + posy0);
			
			yx[0] = accx0+getRand(-0.5,0.5);
			yy[0] = accy0+getRand(-0.5,0.5);
			
			float lastvx, lastvy;
			if (i < 500 || i%1==0) {
				yx[1] = velx1+getRand(-1.0,1.0);
				yy[1] = vely1+getRand(-1.0,1.0);
				lastvx = yx[1];
				lastvy = yy[1];
			}
			else {
				lastvx += yx[0];
				lastvy += yy[0];
				yx[1] = lastvx;
				yy[1] = lastvy;
			}

			float lastpx,lastpy;
			if (i < 500 || i%20==0) {
				yx[2] = posx0+getRand(-1.5,1.5);
				yy[2] = posy0+getRand(-1.5,1.5);
				lastpx = yx[2];
				lastpy = yy[2];
			}
			else {
				lastpx += .5*yx[0]*time_v*time_v + yx[1]*time_v;
				lastpy += .5*yy[0]*time_v*time_v + yy[1]*time_v;
				yx[2] = lastpx;
				yy[2] = lastpy;
			}
			
			printf("%f %f %f %f %f %f ",yx[0],yy[0],yx[1],yy[1],yx[2],yy[2]);
			
			ux[0] = 0;//d*xcomponent;
			uy[0] = 0;//d*ycomponent;
			
			if (i < 500) {
				kalman(Px,Kx,yx,xx,ux,1);
				kalman(Py,Ky,yy,xy,uy,1);
			}
			else if (i >= 500 && (i % 10 == 0)) {
				kalman(Px,Kx,yx,xx,ux,1);
				kalman(Py,Ky,yy,xy,uy,1);
			}
			else {
				kalman(Px,Kx,yx,xx,ux,0);
				kalman(Py,Ky,yy,xy,uy,0);
			}

			
			printf("%f %f %f %f %f %f ",accx0,accy0,velx1,vely1,posx0,posy0);
			//printf("\t\t%d\n",i);
			printf("%f %f %f %f %f %f\n",xx[0],xy[0],xx[1],xy[1],xx[2],xy[2]);
			//printf("%f %f %f %f %f %f\n",0.,0.,0.,0.,0.,0.);
			
			velx0 = velx1;
			vely0 = vely1;
		}
	}	
	printMatrix(Px);
	printMatrix(Py);
}

